/*
 * create by Jinghui Wang on 2025-2-13
 * This file heavily references the repository of Xiang Gao, https://github.com/gaoxiang12/slambook
 */

#include <iostream>
#include <chrono> // 用于测试运算时间
#include <Eigen/Core> // Eigen 核心库
#include <Eigen/Dense> // 稠密矩阵代数运算, 例如逆矩阵, 特征值等
#include <unistd.h>

int main(int argc, char **argv) {

    /********************* 创建矩阵 **********************/

    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    Eigen::Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>， d 代表 double， f 代表 float
    Eigen::Vector3d v_3d;
    Eigen::Vector3f v_3f; // 而这个是 float 类型

    // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); // 初始化为零，如果不初始化，是随机值，由编译器决定

    // 模板元编程要求在编译时能够确定数据类型，当矩阵维数只有运行时才能确定时，需要一个动态标识符送进去
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x; // 更简单的写法
    Eigen::Matrix3Xd matrix_3x; // 3x 表示行固定，列动态

    /********************* 访问元素 **********************/
    // Eigen 中重载了输出运算符
    // 输入数据
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // matrix_23 << 1, 2, 3, 4, 5; // 维度不匹配，运行时错误
    std::cout << matrix_23 << std::endl;
    // printf(matrix_23); // printf()只支持字符串，当然不能这么输出

    // 用 (row, col) 来访问矩阵中的元素
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 3; j++)
            std::cout << matrix_23(i, j) << "\t";
        std::cout << std::endl;
    }

    /********************* 矩阵运算 **********************/
    v_3d << 3, 2, 1;
    v_3f << 4, 5, 6;
    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样会有编译错误，而且报错信息不直观
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    // 正确的方法是显式转换
    const auto result1 = matrix_23.cast<double>() * v_3d;
    std::cout << "result 1:\n" << result1 << std::endl;

    const auto result2 = matrix_23 * v_3f;
    std::cout << "result 2:\n" << result2 << std::endl;

    // 同样你不能搞错矩阵的维度,像这样会有编译错误，而且报错信息不直观
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    // 四则运算用+=*/即可，eigen内部都是重载好的
    matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
    std::cout << "random matrix:\n" << matrix_33 << "\n" <<std::endl;

    std::cout << matrix_33.transpose() << "\n" << std::endl; // 转置
    std::cout << matrix_33.sum() << "\n" << std::endl;       // 各元素和
    std::cout << matrix_33.trace() << "\n" << std::endl;     // 迹
    std::cout << 10*matrix_33 << "\n" << std::endl;          // 数乘
    std::cout << matrix_33.inverse() << "\n" << std::endl;   // 逆
    std::cout << matrix_33.determinant() << "\n" << std::endl;    // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    auto start_time = std::chrono::steady_clock::now();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
    auto end_time = std::chrono::steady_clock::now();
    auto time_used = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(end_time - start_time);
    std::cout << "Eigen values = \n" << eigen_solver.eigenvalues() << "\n" << std::endl;
    std::cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << "\n" << std::endl;
    std::cout << "time used in Eigen solver: " << time_used.count() << " us" << std::endl;


    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程, 直接求逆自然是最直接的，但是求逆运算量大
    const int kMatrixSize = 50;
    Eigen::Matrix<double, kMatrixSize, kMatrixSize> matrix_NN = Eigen::MatrixXd::Random(kMatrixSize, kMatrixSize);
    Eigen::Matrix<double, kMatrixSize, 1> v_Nd = Eigen::MatrixXd::Random(kMatrixSize, 1);

    // 直接求逆
    auto start_time2 = std::chrono::steady_clock::now();
    auto x = matrix_NN.inverse()*v_Nd;
    usleep(1); // 如果不加上，编译器会优化掉上面这行代码
    auto end_time2 = std::chrono::steady_clock::now();
    auto time_used2 = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(end_time2 - start_time2);

    std::cout << "time used in normal inverse is " << time_used2.count()-1 << " us" << std::endl;

    // 通常用矩阵分解来求，例如QR分解，速度会快很多
    // 注意，如果不开编译器优化（比如Debug类型编译），就会非常慢。受优化登记影响很大
    auto start_time3 = std::chrono::steady_clock::now();
    volatile auto x_QR = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    auto end_time3 = std::chrono::steady_clock::now();
    auto time_used3 = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(end_time3 - start_time3);
    std::cout << "time used in QR decomposition is " << time_used3.count() << " us" << std::endl;

    return 0;
}